#*- coding: utf-8 -*-
import threading
import time
import socket
import rospy
import moveit_msgs.msg
from transform import *
from ti5_kinematic import Ti5Kinematic
#from Speed.msg import Speed   # 将订阅str_message话题，自定义消息类型Speed::Speed
from std_msgs.msg import String
import moveit_commander
#import moveit_msgs.msg
from moveit_msgs.msg import *
import numpy
import geometry_msgs.msg
import matplotlib.pyplot as plt
import trajectory_msgs.msg as tm
Vc=-1 #需要在ROS和socket中传递或使用的变量设为全局变量
connections = []
total_connections = 0

class Server(threading.Thread):
    #def __init__(self):
    def __init__(self, socket, address, id, name, signal):
        threading.Thread.__init__(self)
        self.socket = socket
        self.address = address
        self.id = id
        self.name = name
        self.signal = signal

        self.pub = rospy.Publisher('ik_result', moveit_msgs.msg.DisplayTrajectory, queue_size=20)
        self.arm_kinematic = Ti5Kinematic()
        self.last_position=[0,0,0,0,0,0]
        self.tmp_last_position=[0,0,0,0,0,0]
        self.start_point=[0,0,0,0,0,0]
        self.tmp_start_point=[0,0,0,0,0,0]
        self.n8 = numpy.linspace(1,-1,5)
        self.total_end_joint_position = list()

        group_name = "armgroup"
        self.group = moveit_commander.MoveGroupCommander(group_name)
    def __str__(self):
        return str(self.id) + " " + str(self.address)



    def filter_radom_point(self,end_list):
        end_list.insert(0,end_list[0])
        end_list.insert(0,end_list[0])
        end_list.insert(-1,end_list[-1])
        end_list.insert(-1,end_list[-1])
        #for iiiii in end_list:
        #    print(str(iiiii))
        end_list=end_list[:]

        tmp_list =end_list[:]
        le = len(end_list)
        static_num =0
        print("starting filter radom point")

        del_list=list()

        for i in range(2,le-1):
            if static_num>7:
                tmp_list=list()
                print("del random>7!!!!!!!!!!!!!!!!!!!!!"*2)
                break
            for ii in range(6):
                target_point=end_list[i][ii]
                if end_list[i-1][ii]>end_list[i][ii] and end_list[i][ii]<end_list[i+1][ii]:
                    if end_list[i-2][ii]<end_list[i-1][ii] or end_list[i+1][ii]>end_list[i+2][ii]:
                        del_list.append(i)
                        static_num+=1
                        break
                elif end_list[i-1][ii]<end_list[i][ii] and end_list[i][ii]>end_list[i+1][ii]:
                    if end_list[i-2][ii]>end_list[i-1][ii] or end_list[i+1][ii]<end_list[i+2][ii]:
                        del_list.append(i)
                        static_num+=1
                        break
        print("del_list:"+str(del_list))
        if del_list:
            for d1 in reversed(del_list):
                try:
                    del tmp_list[d1-2]
                except:
                    pass
        if len(tmp_list)>4:
            del tmp_list[0]
            del tmp_list[0]
            del tmp_list[-1]
            del tmp_list[-1]
        print("totle len:"+str(len(tmp_list)))

        #print("end filter radom point")
        return tmp_list





    def InfoCallback(self,msg):#ROS订阅的回调函数
        global Vc
        Vc=msg.carspeed   

    def save_end_joint_position(self,tmp_joint):
        len_position = len(self.total_end_joint_position)>2
        if len_position>2:
            ss =1#1 is ok  2 is no 
            for t11 in range(6):
                if self.total_end_joint_position[len_position-2][t11]>self.total_end_joint_position[len_position-1][t11] and self.total_end_joint_position[len_position-1][t11]>tmp_joint[t11]:
                    ss=1
                elif self.total_end_joint_position[len_position-2][t11]<self.total_end_joint_position[len_position-1][t11] and self.total_end_joint_position[len_position-1][t11]<tmp_joint[t11]:
                    ss=1
                else:
                    ss = 0
                    break
            if ss ==1:
                self.total_end_joint_position.append(tmp_joint)
        
            self.total_end_joint_position.append(tmp)



    def send_info(self,client): #SOCKET的发送函数
        global Vc
        while(1):
            if Vc>=0:
                hvjson=Vc #将vc发送出去
                client.sendall(hvjson.encode("utf-8"))
                time.sleep(1)

    def list_get_min(self,li):
        each_cha = 0
        for i in range(6):
            sta = 0
            if li[i]>0 and self.last_position[i]>0:
                sta=1
            if li[i]<0 and self.last_position[i]<0:
                sta=1
            if sta==1:
                each_cha += abs(self.last_position[i]-li[i])
            elif sta==0:
                each_cha += abs(self.last_position[i])+abs(li[i])
            #print("li:"+str(li[i])+" last_position[i]"+str(self.last_position[i])+"each_cha"+str(each_cha))
            #print(str(each_cha))
        return each_cha

    def change_end_traj(self,restart_num,end_point,end_traj_point_list):
        for i in range(6):
            if self.start_point[i]>0 and end_point[i]>0:
                per_distance  = abs(self.start_point[i]-end_point[i])/20
            elif self.start_point[i]<0 and end_point[i]<0:
                per_distance  = abs(self.start_point[i]-end_point[i])/20
            else:
                per_distance  = abs(self.start_point[i])+abs(end_point[i])
                per_distance = per_distance/20
            li_tmp = list()
            if self.start_point[i]>end_point[i]:
                if i==3333:
                    for ii in range(retart_num,20):
                        li_tmp.append(self.start_point[i]-(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                else:
                    for ii in range(restart_num,20):
                        tt=ii+1
                        if i<3:
                            end_traj_point_list[i][ii]=self.start_point[i]-per_distance*tt
                            #li_tmp.append(self.start_point[i]-per_distance*tt)
                        else:
                            end_traj_point_list[i][ii]=self.start_point[i]-per_distance*20

                            #li_tmp.append(self.start_point[i]-per_distance*20)
            else:
                if i==33333:
                    for ii in range(restart_num,20):
                        li_tmp.append(self.start_point[i]+(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                else:
                    for ii in range(restart_num,20):
                        tt=ii+1
                        if i<3:
                            end_traj_point_list[i][ii]=self.start_point[i]+per_distance*tt
                            #li_tmp.append(self.start_point[i]+per_distance*tt)
                        else:
                            end_traj_point_list[i][ii]=self.start_point[i]+per_distance*20
                            #li_tmp.append(self.start_point[i]+per_distance*20)

                                #print(str((1-self.n[ii])**3/20)+"^^^"+str(self.start_point[i]-per_distance*tt))
                                #li_tmp.append(self.start_point[i]+per_distance*tt+(1-self.n[ii])**3/20)


            #end_traj_point_list.append(li_tmp)
        return end_traj_point_list



    def run(self): #SOCKET的接收函数
        global data
        global dd
        while self.signal:

            #try:
            end_point=[0,0,0,0,0,0]

            if 1<2:
                try:
                    info=self.socket.recv(320)
                except:
                    print("Client " + str(self.address) + " has disconnected")
                    self.signal = False
                    connections.remove(self)
                    break

                data=info.decode("utf-8")   #接收到的数据
            proc =0
            if data:
                if data[0]=="R":

                    data = data[1:]
                    li_2 = data.split("_")
                    #li_2 = li_1[0].split(" ")
                    try:
                        li = map(float,li_2)
                        print("start_rpy: "+str(self.start_point))
                        print("last_position:"+str(self.last_position))
                        print("tcp_recieved:"+str(li))
                        li[0]=li[0]*700
                        li[1]=li[1]*700
                        li[2]=li[2]*700
                        end_point=li
                        proc = 1
                    except:
                        proc =0
                        end_point=list()
                elif data[0]=="J":
                    data = data[1:]
                    li_1=data.split("_")
                    li = map(float,li_1)
                    print("tcp_recieved:"+str(li))
                    end_point=list()
                    print("@@@"*12)
                    proc =2


                if proc==1:
                    self.total_end_joint_position=list()
                    tmp_start_point = self.start_point[:]
                    tmp_last_position = self.last_position[:]
                    print("!!!!"*20)
                    end_traj_point_list =list()
                    for i in range(6): 
                        #print(str(i)*20)
                        if self.start_point[i]>0 and end_point[i]>0:
                            per_distance  = abs(self.start_point[i]-end_point[i])/20
                        elif self.start_point[i]<0 and end_point[i]<0:
                            per_distance  = abs(self.start_point[i]-end_point[i])/20
                        else:
                            per_distance  = abs(self.start_point[i])+abs(end_point[i])
                            per_distance = per_distance/20
                        li_tmp = list()
                        if self.start_point[i]>end_point[i]:
                            if i==3333:
                                for ii in range(20):
                                    li_tmp.append(self.start_point[i]-(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(20):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.start_point[i]-per_distance*tt)
                                    else:

                                        li_tmp.append(self.start_point[i]-per_distance*20)
                        else:    
                            if i==33333:
                                for ii in range(20):
                                    li_tmp.append(self.start_point[i]+(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(20):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.start_point[i]+per_distance*tt)
                                    else:
                                        li_tmp.append(self.start_point[i]+per_distance*20)


                        
                        end_traj_point_list.append(li_tmp)
                    for iiiii in range(20):
                        l = list()
                        l.append(round(end_traj_point_list[0][iiiii],4))
                        l.append(round(end_traj_point_list[1][iiiii],4))
                        l.append(round(end_traj_point_list[2][iiiii],4))
                        l.append(round(end_traj_point_list[3][iiiii],4))
                        l.append(round(end_traj_point_list[4][iiiii],4))
                        l.append(round(end_traj_point_list[5][iiiii],4))
                        

                    temp_list = list()
                    tot_list =list()
                    le = 0 #static total success
                    for iii in range(20):
                        #print(str(end_traj_point_list[0][iii])+str(end_traj_point_list[1][iii])+str(end_traj_point_list[2][iii])+str(end_traj_point_list[3][iii])+str(end_traj_point_list[4][iii])+str(end_traj_point_list[5][iii]))
                        pose = Pose()    
                        pose.set_position(end_traj_point_list[0][iii],end_traj_point_list[1][iii],end_traj_point_list[2][iii])
                        pose.set_euler_angle(end_traj_point_list[3][iii],end_traj_point_list[4][iii],end_traj_point_list[5][iii])

                        candi_joint_angle_list_final = self.arm_kinematic.inverse_kinematic(pose)
                        #tot_list.append(candi_joint_angle_list_final)
                        if candi_joint_angle_list_final:
                            cha=10
                            res_select=0 #the result min total_cha list's number
                            numnum = 0
                            for t in candi_joint_angle_list_final:
                                now_cha = self.list_get_min(t)
                                #print("list[]:"+str(iii)+"   numnum:"+str(numnum)+"   nowcha:"+str(now_cha))
                                if now_cha<cha:
                                    if -3.15<candi_joint_angle_list_final[numnum][0]<3.15 and -3.15<candi_joint_angle_list_final[numnum][1]<3.15 and -3.15<candi_joint_angle_list_final[numnum][2]<3.15 and -3.15<candi_joint_angle_list_final[numnum][3]<3.15 and -3.15<candi_joint_angle_list_final[numnum][4]<3.15 and -3.15<candi_joint_angle_list_final[numnum][5]<3.15 :

                                        cha=now_cha
                                        res_select = numnum



                                numnum+=1



                            #print(str(candi_joint_angle_list_final[res_select]))    
                            #if cha<3 or iii==0:
                            if cha<3 or self.start_point[0]==0:

                                #print("last:"+str(end_traj_point_list[0][iii])+"last:"+str(end_traj_point_list[1][iii])+"last:"+str(end_traj_point_list[2][iii]))
                                le+=1
                                self.last_position = candi_joint_angle_list_final[res_select]
                                self.total_end_joint_position.append(self.last_position)

                                self.start_point[0]=pose.x    
                                self.start_point[1]=pose.y    
                                self.start_point[2]=pose.z    
                                self.start_point[3]=pose.roll    
                                self.start_point[4]=pose.pitch 
                                self.start_point[5]=pose.yaw    

                            else:
                                #print("didnt resolve ,will jump out this round")
                                # 1st round 1st 1st
                                #num of 20 &endpoint
                                jump_point = list()
                                for t1 in range(6):
                                    for mi in range(1,6):
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]+=alter_base[t1]*0.12*mi
                                        jump_point.append(alter_base)
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]-=alter_base[t1]*0.12*mi
                                        jump_point.append(alter_base)
                                for t1 in range(5):
                                    for mi in range(1,6):  #alter_volum_X_multi
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]+=alter_base[t1]*0.12*mi
                                        alter_base[t1+1]+=alter_base[t1+1]*0.12*mi
                                        jump_point.append(alter_base)
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]-=alter_base[t1]*0.12*mi
                                        alter_base[t1+1]-=alter_base[t1+1]*0.12*mi
                                        jump_point.append(alter_base)
                                for t1 in range(4):
                                    for mi in range(1,6):  #alter_volum_X_multi
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]+=alter_base[t1]*0.12*mi
                                        alter_base[t1+1]+=alter_base[t1+1]*0.12*mi
                                        alter_base[t1+2]+=alter_base[t1+2]*0.12*mi
                                        jump_point.append(alter_base)
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]-=alter_base[t1]*0.12*mi
                                        alter_base[t1+1]-=alter_base[t1+1]*0.12*mi
                                        alter_base[t1+2]-=alter_base[t1+2]*0.12*mi
                                        jump_point.append(alter_base)

                                for t1 in range(3):
                                    for mi in range(1,6):  #alter_volum_X_multi
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]+=alter_base[t1]*0.12*mi
                                        alter_base[t1+1]+=alter_base[t1+1]*0.12*mi
                                        alter_base[t1+2]+=alter_base[t1+2]*0.12*mi
                                        alter_base[t1+3]+=alter_base[t1+3]*0.12*mi
                                        jump_point.append(alter_base)
                                        alter_base=[0,0,0,0,0,0]
                                        alter_base[0] = end_traj_point_list[0][iii]
                                        alter_base[1] = end_traj_point_list[1][iii]
                                        alter_base[2] = end_traj_point_list[2][iii]
                                        alter_base[3] = end_traj_point_list[3][iii]
                                        alter_base[4] = end_traj_point_list[4][iii]
                                        alter_base[5] = end_traj_point_list[5][iii]
                                        alter_base[t1]-=alter_base[t1]*0.12*mi
                                        alter_base[t1+1]-=alter_base[t1+1]*0.12*mi
                                        alter_base[t1+2]-=alter_base[t1+2]*0.12*mi
                                        alter_base[t1+3]+=alter_base[t1+3]*0.12*mi
                                        jump_point.append(alter_base)


                                t5 = 0
                                for jp in jump_point:
                                    t5+=1
                                    pose = Pose()
                                    pose.set_position(jp[0],jp[1],jp[2])
                                    pose.set_euler_angle(jp[3],jp[4],jp[5])
                                    candi_joint_angle_list_final = self.arm_kinematic.inverse_kinematic(pose)
                                    if candi_joint_angle_list_final:
                                        #print("!"*30)
                                        #for t3 in candi_joint_angle_list_final: 
                                        cha=10
                                        res_select=0 #the result min total_cha list's number
                                        numnum = 0
                                        for t in candi_joint_angle_list_final:
                                            now_cha = self.list_get_min(t)
                                            if now_cha<cha:
                                                if -3.15<candi_joint_angle_list_final[numnum][0]<3.15 and -3.15<candi_joint_angle_list_final[numnum][1]<3.15 and -3.15<candi_joint_angle_list_final[numnum][2]<3.15 and -3.15<candi_joint_angle_list_final[numnum][3]<3.15 and -3.15<candi_joint_angle_list_final[numnum][4]<3.15 and -3.15<candi_joint_angle_list_final[numnum][5]<3.15 :

                                                    cha=now_cha
                                                    res_select = numnum
                                            numnum+=1
                            #print(str(candi_joint_angle_list_final[res_select]))
                                        #if cha<4.5 or iii==0:
                                        if cha<4.5 or self.start_point[0]==0:
                                            #print("JJJ"*20+str(jp))
                                            #print("next:"+str(end_traj_point_list[0][iii])+"next:"+str(end_traj_point_list[1][iii])+"next:"+str(end_traj_point_list[2][iii]))
                                            #print(str(tmp_start_point))
                                            le+=1
                                            #print("*"*20+str(t5-1))
                                            self.last_position = candi_joint_angle_list_final[res_select]
                                            self.total_end_joint_position.append(self.last_position)
                                            #print("pre_after success:"+str(self.last_position))
                                            self.start_point[0]=pose.x    
                                            self.start_point[1]=pose.y    
                                            self.start_point[2]=pose.z    
                                            self.start_point[3]=pose.roll    
                                            self.start_point[4]=pose.pitch 
                                            self.start_point[5]=pose.yaw    
                                            #print(str(tmp_start_point))
                                            
                                            #save joint_position
                                            self.save_end_joint_position(self.last_position)


                                            #change_end_traj
                                            
                                            
                                            self.change_end_traj(iii,end_point,end_traj_point_list)
                                            break


                            #print("####"+str(self.last_position))
                            #print("                  "),

                            #print(" ")

                    #print("$:"+str(le))
                    if le<7:
                        self.start_point = tmp_start_point[:]
                        self.last_position = tmp_last_position[:]
                    else:
                        end_end_end = self.filter_radom_point(self.total_end_joint_position)

                        #for e1 in self.total_end_joint_position:
                        if end_end_end:
                            dp =DisplayTrajectory()
                            rt = RobotTrajectory()

                            jt=tm.JointTrajectory()
                            for e1 in end_end_end:
                                jtp=tm.JointTrajectoryPoint()
                                jtp.positions = e1
                                jt.points.append(jtp)

                            rt.joint_trajectory=jt
                            dp.trajectory.append(rt)
                        
                            end_str_tcp = "J"+str(self.last_position[0])+"_"+str(self.last_position[1])+"_"+str(self.last_position[2])+"_"+str(self.last_position[3])+"_"+str(self.last_position[4])+"_"+str(self.last_position[5])+";"

                            i9=self.socket.send(end_str_tcp.encode("utf-8"))

                            self.pub.publish(dp)  #send to rostopic  <<charter>>
                        else:
                            self.start_point = tmp_start_point[:]
                            self.last_position = tmp_last_position[:]
                            self.socket.send("ik fail")

                elif proc ==2:
                    #print("pre_start:"+str(self.start_point))
                    #print("pre_last:"+str(self.last_position))
                    end_traj_point_list=list()
                    #print()
                    for i in range(6): 
                        #print(str(i)*20)
                        if self.last_position[i]>0 and li[i]>0:
                            per_distance  = abs(self.last_position[i]-li[i])/20
                        elif self.last_position[i]<0 and li[i]<0:
                            per_distance  = abs(self.last_position[i]-li[i])/20
                        else:
                            per_distance  = abs(self.last_position[i])+abs(li[i])
                            per_distance = per_distance/20
                        li_tmp = list()
                        if self.last_position[i]>li[i]:
                            if i==3333:
                                for ii in range(20):
                                    li_tmp.append(self.last_position[i]-(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(20):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.last_position[i]-per_distance*tt)
                                    else:

                                        li_tmp.append(self.last_position[i]-per_distance*20)
                        else:    
                            if i==33333:
                                for ii in range(20):
                                    li_tmp.append(self.last_position[i]+(per_distance*tt+abs(1-abs(self.n8[ii])/10)))
                            else:
                                for ii in range(20):
                                    tt=ii+1
                                    if i<30:
                                        li_tmp.append(self.last_position[i]+per_distance*tt)
                                    else:
                                        li_tmp.append(self.last_position[i]+per_distance*20)


                        
                        end_traj_point_list.append(li_tmp)
                    end_end_end=list()
                    for iiiii in range(20):
                        l = list()
                        l.append(round(end_traj_point_list[0][iiiii],4))
                        l.append(round(end_traj_point_list[1][iiiii],4))
                        l.append(round(end_traj_point_list[2][iiiii],4))
                        l.append(round(end_traj_point_list[3][iiiii],4))
                        l.append(round(end_traj_point_list[4][iiiii],4))
                        l.append(round(end_traj_point_list[5][iiiii],4))
                        end_end_end.append(l)

                    #for i in end_end_end:
                    #print(str(i))
                    ep =[0,0,0,0,0,0] 
                    target_pose = self.arm_kinematic.forward_kinematic_v2(end_end_end[-1])
                    #print("target:"+str(target_pose))

                    ep[0]=target_pose.x/700
                    ep[1]=target_pose.y/700
                    ep[2]=target_pose.z/700
                    ep[3]=target_pose.roll
                    ep[4]=target_pose.pitch
                    ep[5]=target_pose.yaw
                    self.start_point= ep


                    print("end_end_end[-1]:"+str(end_end_end[-1]))
                    self.last_position =end_end_end[-1]
                    print("last_position: "+str(self.last_position))

                    if 1<2:
                        dp =DisplayTrajectory()
                        rt = RobotTrajectory()

                        jt=tm.JointTrajectory()
                        for e1 in end_end_end:
                            jtp=tm.JointTrajectoryPoint()
                            jtp.positions = e1
                            jt.points.append(jtp)

                        rt.joint_trajectory=jt
                        dp.trajectory.append(rt)
                        
                        end_str_tcp = "J"+str(self.last_position[0])+"_"+str(self.last_position[1])+"_"+str(self.last_position[2])+"_"+str(self.last_position[3])+"_"+str(self.last_position[4])+"_"+str(self.last_position[5])+";"

                        i9=self.socket.send(end_str_tcp.encode("utf-8"))

                        self.pub.publish(dp)  #send to rostopic  <<charter>>
                else:
                    print("tcp error pass")






#Wait for new connections
def newConnections(socket):
    while True:
        sock, address = socket.accept()
        global total_connections
        connections.append(Server(sock, address, total_connections, "Name", True))
        connections[len(connections) - 1].start()
        print("New connection at ID " + str(connections[len(connections) - 1]))
        total_connections += 1



if __name__ == '__main__':
#先开启了ROS

    rospy.init_node('position_pubscriber', anonymous=True)   # ROS节点初始化

    #ser=Server()
    host = input("Host: ")
    port = int(input("Port: "))
    sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
    sock.bind((host, port))
    sock.listen(5)
    #Create new thread to wait for connections
    newConnectionsThread = threading.Thread(target = newConnections, args = (sock,))
    newConnectionsThread.start()


    #rospy.Subscriber("str_message", Speed, InfoCallback)   # 创建一个Subscriber，订阅名为/str_message的topic，注册回调函数InfoCallback
 
 #再创建了SOCKET       

    #threads = []
    #t1 = threading.Thread(target=ser.send_info, args=(client,))  #开启线程
    #threads.append(t1)
    #t2 = threading.Thread(target=ser.recv_info, args=(client,))
    #threads.append(t2)
    #for t in threads:
    #    print "1212121"
    #    t.start()
    #    time.sleep(1)


